/*
 * CurrentRobotState.java
 *
 * Created on July 27, 2001, 11:09 AM
 */

package edu.wsu.KheperaSimulator;

/**
 *
 * @author  steve
 * @version 
 */
public class CurrentRobotState
{
    private Sensor[] sensors;
    private int armState, gripperState;
    private Motor motor;
    private int resistivity;
    private boolean objPresent;
    private boolean objHeld;


	public boolean armChanged;
	public boolean gripChanged;
	public boolean motorsChanged;
	
    
    public int sessionStatus;
    public String waitTime;
    
    
    private int lStuck;
    private int rStuck;
    private long lPos;
    private long rPos;

    private static final int STUCK_LIMIT = 20;

	public CurrentRobotState()
    {
    	sessionStatus = ClientConfiguration.WAITING;
    	waitTime = "in ----";

		rStuck = 0;
		lStuck = 0;
		lPos = 0;
		rPos = 0;
        
        armState = KSGripperStates.ARM_UP;
        gripperState = KSGripperStates.GRIP_CLOSED;
        resistivity = 0;
        objPresent = false;
        objHeld = false;
        motor = new Motor();
        sensors = new Sensor[8];
        
        initSensors();
    }


  

    private void initSensors()
    {
        for(int i = 0;i < 8;i++)
            sensors[i] = new Sensor();
    }


	// =========================================================================
	// ===                          	========================================
	// ===		Mutator Methods			========================================
	// ===                          	========================================
	// =========================================================================
    
    
	public void setArmState(int arm)
    { 
        if( arm == KSGripperStates.ARM_UP || arm == KSGripperStates.ARM_DOWN )
        {
			armState = arm;
			armChanged = true;
		}

		else
		{
			System.out.println("An illegal arm state was used.  Check the");
			System.out.println("controller source code to make sure you're");
			System.out.println("doing things properly.\n");
			
			System.out.println("Session ended.");
			
			System.exit(0);
		}
    }
      
   
    public void setGripperState(int grip) 
    { 
        if( grip == KSGripperStates.GRIP_OPEN || grip == KSGripperStates.GRIP_CLOSED )
        {
			gripperState = grip;
			gripChanged = true;
		}
		
		else
		{
			System.out.println("An illegal gripper state was used.  Check the");
			System.out.println("controller source code to make sure you're");
			System.out.println("doing things properly.\n");
			
			System.out.println("Session ended.");
			
			System.exit(0);	
		}
    }


    public void setMotorSpeeds( int left, int right )
    {
    	motor.setMotorSpeeds( left, right );
    	motorsChanged = true;
	}
	
	
	public void setLeftMotorSpeed( int speed )
	{
		motor.setLeftMotorSpeed( speed );
		motorsChanged = true;
	}
	
	
	public void setRightMotorSpeed( int speed )
	{
		motor.setRightMotorSpeed( speed );
		motorsChanged = true;
	}
    
    public void setMotorPositions( long left, long right )
    {
    	motor.setMotorPositions( left, right );
    }
    
    public void postSensorValues(Sensor[] s)
    {
    	sensors = s;
    }
  
   
    public void postResistivity(int rVal)
    {
    	resistivity = rVal;
    }
    
    
    public void postObjectPresent(boolean objPresent)
    { 
        this.objPresent = objPresent;
    }

	
	
	// =========================================================================
	// ===                          	========================================
	// ===		Accessor Methods		========================================
	// ===                          	========================================
	// =========================================================================
    
    public long getLeftPosition()
    {
    	return motor.leftPosition;
    }
    
    
    public long getRightPosition()
    {
    	return motor.rightPosition;
    }
    
    public boolean isStuck()
    {
		if( motor.leftSpeed != 0 )
		{
			if( lPos == motor.leftPosition )
				lStuck++;
			else
				lStuck = 0;
		}

		if( motor.rightSpeed != 0 )
		{
			if( rPos == motor.rightPosition )
				rStuck++;
			else
				rStuck = 0;
		}


		lPos = motor.leftPosition;
		rPos = motor.rightPosition;

		if( rStuck > STUCK_LIMIT || lStuck > STUCK_LIMIT )
			return true;

		else
			return false;
    }


	public int getArmState() 
    {
    	return armState; 
    }
       
    
    public int getGripperState()
    {
    	return gripperState; 
    }
    
    
    public Motor getMotor()
    {
    	return motor; 
    }
    
    
    public Motor getMotorState()
    {
    	return motor; 
    }
    
    
    public Sensor[] getSensorValues()
    {
    	return sensors;
    }


	public int getResistivity()
    {
    	return resistivity;
    } 
   

 	public boolean isObjectPresent()
    {
    	return objPresent;
    }
   
   
	public boolean isObjectHeld()
    {
    	return objHeld;
    }
    
    
    

    

}





